# PA_SERVO.py: 控制PCA9685驱动的舵机电机实现文件
from machine import SoftI2C, Pin
import ustruct
import time
from setcfg import DAT

class PCA9685:
    def __init__(self, i2c, address=0x40):
        # 初始化PCA9685对象，传入I2C对象和地址
        self.i2c = i2c
        self.address = address
        self.status=0
        #self.reset()

    def _write(self, address, value):
        # 写入单个字节到指定寄存器
        try:
          self.i2c.writeto_mem(self.address, address, bytearray([value]))
          return 1
        except Exception as e:
          print("PCA9685 写入异常:"+str(e))
          return 0

    def _read(self, address):
        # 从指定寄存器读取单个字节
        try:
          return self.i2c.readfrom_mem(self.address, address, 1)[0]
        except Exception as e:
          print("PCA9685 读取异常:"+str(e))
          return 0

    def reset(self):
        # 复位PCA9685
        if self._write(0x00, 0x00)==1: # 设置Mode1寄存器为默认值
            self.status=1           
        else:
            self.status=0
            print("PCA9685启动失败!")

    def freq(self, freq=None):
        if self.status==0:
            print("PCA9685 已经停止 on freq")
            return
        # 获取或设置PWM频率
        if freq is None:
            return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
        prescale = int(25000000.0 / 4096.0 / freq + 0.5)
        old_mode = self._read(0x00) # 读取当前Mode1寄存器值
        self._write(0x00, (old_mode & 0x7F) | 0x10) # 进入睡眠模式
        self._write(0xfe, prescale) # 设置预分频器
        self._write(0x00, old_mode) # 退出睡眠模式
        time.sleep_us(5)
        self._write(0x00, old_mode | 0xa1) # 自动递增模式

    def pwm(self, index, on=None, off=None):
        if self.status==0:
            #print("PCA9685 已经停止 on pwm")
            return
        # 获取或设置PWM占空比
        if on is None or off is None:
            data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
            return ustruct.unpack('<HH', data)
        data = ustruct.pack('<HH', on, off)
        self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data)

    def duty(self, index, value=None, invert=False):
        # 获取或设置PWM占空比（以4095为最大）
        if value is None:
            pwm = self.pwm(index)
            if pwm == (0, 4096):
                value = 0
            elif pwm == (4096, 0):
                value = 4095
            else:
                value = pwm[1]
            if invert:
                value = 4095 - value
            return value
        if not 0 <= value <= 4095:
            raise ValueError("Out of range")
        if invert:
            value = 4095 - value
        if value == 0:
            self.pwm(index, 0, 4096)
        elif value == 4095:
            self.pwm(index, 4096, 0)
        else:
            self.pwm(index, 0, value)

class Servos:
    def __init__(self, i2c, address=DAT["servo"]["addr"], freq=DAT["servo"]["freq"], min_us=DAT["servo"]["min_us"], max_us=DAT["servo"]["max_us"],  # 根据舵机参数自行设置
                 degrees=180):
        # 初始化Servos对象，传入I2C对象、地址、频率、最小/最大微秒值、角度范围
        self.period = 1000000 / freq  # 周期时间，单位为微秒
        self.min_duty = self._us2duty(min_us)
        self.max_duty = self._us2duty(max_us)
        self.degrees = degrees
        self.freq = freq
        self.pca9685 = PCA9685(i2c, address)
        #self.pca9685.freq(freq)

    def _us2duty(self, value):
        # 将微秒值转换为PWM占空比
        return int(4095 * value / self.period)

    def position(self, index, degrees=None, radians=None, us=None, duty=None):
        # 设置舵机位置（角度、弧度、微秒值或占空比）
        span = self.max_duty - self.min_duty
        if degrees is not None:
            duty = self.min_duty + span * degrees / self.degrees
        elif radians is not None:
            duty = self.min_duty + span * radians / math.radians(self.degrees)
        elif us is not None:
            duty = self._us2duty(us)
        elif duty is not None:
            pass
        else:
            return self.pca9685.duty(index)
        duty = min(self.max_duty, max(self.min_duty, int(duty)))
        self.pca9685.duty(index, duty)

    def release(self, index):
        # 释放舵机（占空比为0）
        self.pca9685.duty(index, 0)
        
    def stop(self):
        print("停止 pca9685")
        self.pca9685.status=0
    
    def isStart(self):
        return self.pca9685.status
    
    def restart(self):
        print("启动 pca9685")
        self.pca9685.reset()
        self.pca9685.freq(self.freq)
        
    def position_duty(self, index, duty):
       # 设置舵机位置（仅通过占空比）
       int_duty = int(duty)
       self.pca9685.duty(index, int_duty)


print("初始化pca9685 scl=%d sda=%d"%(DAT['pca9685']['scl'],DAT['pca9685']['sda']))
try:
    servos = Servos(SoftI2C(scl=Pin(DAT["pca9685"]["scl"]), sda=Pin(DAT["pca9685"]["sda"]), freq=100000), address=0x40)
except:
    print("pca9685 启动异常！")

def angle(pin_num, degrees):
    # 设置舵机角度
    servos.position(pin_num, degrees)

def release():
    # 释放所有舵机
    for i in range(0, 15):
        servos.release(i)
    servos.stop()

import sys_control
# GPIO 25 打开舵机使能 湃湃狗专用
pin_servo_vol = Pin(DAT["pca9685"]['power_pin'], Pin.OUT)
pin_servo_vol.value(0)

def pca9685_OnOff(statue):
    #控制PCA9685电源开关
    if statue == 1:
      if pin_servo_vol.value() != 1:
        pin_servo_vol.value(1)        
        sys_control.beep_control(0, 0)
        if servos.isStart()==0:
          time.sleep_ms(2000)
          servos.restart()
          
    else:
      if pin_servo_vol.value() != 0:
        release()            
        pin_servo_vol.value(0)
        sys_control.beep_control(350, 500)
        
